#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <geometry_msgs/Twist.h>
#include <agv_msgs/OdometryLite.h>
#include <agv_msgs/PoseWithConfidence.h>
#include <agv_msgs/V_order.h>
#include <agv_msgs/V_node.h>
#include <agv_msgs/AgvAutoStatus.h>
#include <agv_msgs/AGVMode.h>
#include <agv_srvs/QRCalib.h>
#include <agv_srvs/V_order.h>
#include <agv_srvs/SetOperationMode.h>
#include <agv_srvs/V_localOrder.h>
#include <agv_srvs/SetLaserCalib.h>
#include <std_msgs/String.h>
#include <std_msgs/Int32.h>
#include <boost/thread.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf/transform_listener.h>
#include <tf2_ros/transform_listener.h>
#include <Eigen/Dense>
#include <map_reader.h>
#include <numeric>
#include <thread>
#include <chrono>
#include <fstream>
#include <iostream>
#include <agv_msgs/QrCalibPercent.h>

#define PACKAGE_NAME "lidar_calibration"

using namespace std;


struct PoseRecord_{
    double x;
    double y;
    double theta;
};


class QrAutoCalib
{
public:
    QrAutoCalib(ros::NodeHandle& nh):nh_(nh), percent(0){}
    ~QrAutoCalib(){
        //th_end();
        // file1.close();
        // file2.close();
        work_thread->interrupt();
        work_thread->join();
        work_thread.reset();
    }

    bool init();
    bool line_move(int fromNode,int toNode);
    bool line_move(vector<int> node_list);
    double getdistance(geometry_msgs::Pose cur, geometry_msgs::Pose start){
        // ROS_INFO("cur_x:%.4f, cur_y:%.4f, start_x:%.4f, start_y:%.4f", cur.position.x, cur.position.y, start.position.x, start.position.y);
        return sqrt(pow((cur.position.x - start.position.x),2) + pow((cur.position.y - start.position.y),2));
    }
    void landmark_callback(const geometry_msgs::PoseStamped &mark_data);
    void HandleMoveInfoMessage(const agv_msgs::AgvAutoStatus::ConstPtr& msg);
    void InitMap(const ros::WallTimerEvent &timer_event);
    void PublishPercent(const ros::WallTimerEvent &timer_event);
    void RecordPose(const ros::WallTimerEvent &timer_event);
    void HandleModeInfoMessage(const agv_msgs::AGVMode::ConstPtr& msg);
    void pose_callback(const::agv_msgs::PoseWithConfidence& msg);
    void work_cycle();
    bool yaw_cal();
    bool qr_calib_callback(agv_srvs::QRCalib::Request &req, agv_srvs::QRCalib::Response &resp);
    bool initPointsData();
    void OdomSubscribeCallback(const agv_msgs::OdometryLite::ConstPtr& msg);
    bool SavePose(double x,double y,double theta);



private:
    bool record_trigger = false;
    bool record_start = false;
    double rotating_angle;
    bool first_in = false,first_start = false;
    bool calib_success_flag = false;
    int trigger,agv_navigation_type;
    int node0,node1,node2,node3,node4,node5,node6,node7,node8,node9,node10,fromNode,toNode;
    int percent = -1;
    int currNavType = 0;
    int agvModeOld = 0,agvMode = 0;
    int landmark_tag = 0;
    bool moveDir = 0,
         node0_0_called = false,
         node1_0_called = false,
         node2_0_called = false,
         node3_0_called = false,
         node4_0_called = false,
         node5_0_called = false,
         node6_0_called = false,
         node7_0_called = false,
         node8_0_called = false,
         node9_0_called = false,
         node9_1_called = false,
         node8_1_called = false,
         node7_1_called = false,
         node6_1_called = false,
         node5_1_called = false,
         node4_1_called = false,
         node3_1_called = false,
         node2_1_called = false,
         node1_1_called = false,
         node0_1_called = false;
    int moveCycle = 10;
    double cur_theta_ = 0;
    double landmark_theta = 0;
    double diff_threshold = 1;
    double TcX_,TcY_,TcR_;
    int64_t order_num = 0;
    double scale = 0.25;
    double pose_theta_ = 0;
    double diff;
    std::vector<PoseRecord_> pose_list;

    std::vector<int> node_list;


    std::string qr_msg;
    ros::NodeHandle nh_;
    ros::Subscriber pose_sub,moveInfoSub,modeSub,landmarkSub;
    ros::Publisher per_pub;
    ros::ServiceServer server;
    ros::ServiceClient order_client;
    ros::ServiceClient laser_tf_client;
    ros::ServiceClient mode_clinet;
    geometry_msgs::Pose node0_pose;
    geometry_msgs::Pose cur_pose_;
    geometry_msgs::Pose node2_pose_record_1,node2_pose_record_2,node3_pose_record_1,node3_pose_record_2,node4_pose_record_1,node4_pose_record_2;
    geometry_msgs::Twist current_vel;
    geometry_msgs::Pose current_odom_pose;
    geometry_msgs::Pose start_pose;
    geometry_msgs::Pose landmark_pose;
    geometry_msgs::Twist vel_msg;
    ros::Subscriber odometry_sub;    /**< Current velocity from odometry */
    ros::Publisher  vel_pub;         /**< Outgoing velocity commands */

    map_stream_reader map_stream_reader_;
    tf::StampedTransform tfGeom;
    std::vector<::ros::WallTimer> wall_timers_;
    //std::vector<PoseRecord_> pose_list_1,pose_list_2;
    boost::shared_ptr<boost::thread> work_thread;
};

bool QrAutoCalib::init()
{
    // TcX_ = ros::param::param("/dm_reader/TcX", 0.0);
    // TcY_ = ros::param::param("/dm_reader/TcY", 0.0);
    // TcR_ = ros::param::param("/dm_reader/TcR", 0.);
    scale = ros::param::param("/code_read_camera/scale", 0.25);
    pose_theta_ = ros::param::param("/dm_reader/TcR", 0.);
    ros::param::set("/code_read_camera/scale",1);
    ROS_INFO_STREAM("cur TcR is"<<pose_theta_);

    per_pub = nh_.advertise<agv_msgs::QrCalibPercent>("/qrCalib_percent", 1);
    pose_sub = nh_.subscribe("/pose",1, &QrAutoCalib::pose_callback, this);
    landmarkSub = nh_.subscribe("/landmark",1,&QrAutoCalib::landmark_callback, this);
    moveInfoSub = nh_.subscribe("/auto_run_status", 1, &QrAutoCalib::HandleMoveInfoMessage,this);
    modeSub = nh_.subscribe("/agvmode", 1, &QrAutoCalib::HandleModeInfoMessage,this);
    //odometry_sub =nh_.subscribe("/base/controllers/twist_controller/odom", 1, &QrAutoCalib::OdomSubscribeCallback,this);
    vel_pub = nh_.advertise<geometry_msgs::Twist>("/base/twist_mux/keyboard_vel",10);
    server = nh_.advertiseService("/qrCalibration_service",  &QrAutoCalib::qr_calib_callback, this);
    wall_timers_.push_back(nh_.createWallTimer(::ros::WallDuration(0.3),&QrAutoCalib::InitMap,this));
    wall_timers_.push_back(nh_.createWallTimer(::ros::WallDuration(1),&QrAutoCalib::PublishPercent,this));
    //wall_timers_.push_back(nh_.createWallTimer(::ros::WallDuration(0.2),&QrAutoCalib::RecordPose,this));
    ros::service::waitForService("/local_order");
    ros::service::waitForService("/operation_mode");

    order_client = nh_.serviceClient<agv_srvs::V_localOrder>("/local_order");
    mode_clinet = nh_.serviceClient<agv_srvs::SetOperationMode>("/operation_mode");

     if(!initPointsData()){
        ROS_ERROR_STREAM("load xml failed ....");
        return false;
    };
    // file1.open("/home/vensin/1.txt");
    // file2.open("/home/vensin/2.txt");
    work_thread.reset(new boost::thread(&QrAutoCalib::work_cycle, this));
    return true;
}

void QrAutoCalib::HandleMoveInfoMessage(const agv_msgs::AgvAutoStatus::ConstPtr& msg) //agv_msgs::AgvAutoStatus
{
  fromNode = msg->fromNode;
  toNode = msg->toNode;
  auto it = map_stream_reader_.pointsMap.find(toNode);
  auto it_ = map_stream_reader_.pointsMap.find(fromNode);
  //ROS_INFO_STREAM_THROTTLE(0.05,"from node "<<fromNode<<" to node "<< toNode <<"it->second.qrCodeId "<<it->second.qrCodeId<<" landmark_tag "<<landmark_tag<<" record_trigger "<< record_trigger << " record start "<<record_start << " distance is " <<sqrt(pow((it_->second.x - cur_pose_.position.x),2) + pow((it_->second.y - cur_pose_.position.y),2))<<" cur x "<<cur_pose_.position.x<<" qr x "<< it_->second.x);
  //ROS_INFO_STREAM_THROTTLE(0.1,"from node "<<fromNode<<" to node "<< toNode <<"landmark x"<<landmark_pose.position.x<<" landmark y "<<landmark_pose.position.y<<" theta "<< landmark_theta);
  //if(it != map_stream_reader_.pointsMap.end() && it->second.qrCodeId == landmark_tag && record_trigger == false && record_start == true && fromNode != toNode){
  if(it != map_stream_reader_.pointsMap.end() && it->second.qrCodeId == landmark_tag && record_trigger == false && record_start == true && fromNode != toNode){
        SavePose(cur_pose_.position.x,cur_pose_.position.y,cur_theta_);
        ROS_INFO_STREAM("from node "<<fromNode<<" to node "<< toNode <<"landmark x"<<landmark_pose.position.x<<" landmark y "<<landmark_pose.position.y<<" theta "<< landmark_theta);
        record_trigger = true;
    }
  else if(it_ != map_stream_reader_.pointsMap.end() && it_->second.qrCodeId == landmark_tag && record_trigger == true && sqrt(pow((it_->second.x - cur_pose_.position.x),2) + pow((it_->second.y - cur_pose_.position.y),2))>0.25){
        record_trigger = false;
    }
}

void QrAutoCalib::HandleModeInfoMessage(const agv_msgs::AGVMode::ConstPtr& msg)
{
  agvMode = msg->mode;
}
void QrAutoCalib::InitMap(const ros::WallTimerEvent &timer_event)
{
  if(agvMode != 0 && agvModeOld == 0){
    initPointsData();
    pose_theta_ = ros::param::param("/dm_reader/TcR", 0.);
  }
  agvModeOld = agvMode;
}
void QrAutoCalib::landmark_callback(const geometry_msgs::PoseStamped &mark_data)
{
    landmark_tag = mark_data.pose.position.z;
    landmark_pose = mark_data.pose;
    landmark_theta = tf::getYaw(mark_data.pose.orientation);
}

void QrAutoCalib::PublishPercent(const ros::WallTimerEvent &timer_event)
{
    agv_navigation_type = ros::param::param("/AGVParam/agv_navigation_type",0);
    //std_msgs::Int32 per;
    //per.data = percent;
    agv_msgs::QrCalibPercent per;
    per.percent = percent;
    if(calib_success_flag == true){
        per.theta = pose_theta_;
    }
    else{
        per.theta = 65535;
    }

    per_pub.publish(per);
}

bool QrAutoCalib::SavePose(double x,double y,double theta){
    PoseRecord_ pose;
    pose.x = cur_pose_.position.x;
    pose.y = cur_pose_.position.y;
    pose.theta = cur_theta_;
    pose_list.push_back(pose);
    ROS_INFO_STREAM("pose save ! size is "<<pose_list.size()<< " x "<<pose.x<<" y "<<pose.y);
    return true;
}


bool QrAutoCalib::initPointsData()
{
    if(!map_stream_reader_.load_map_xmlfile()){
        ROS_ERROR_STREAM("can not load points!");
        return false;
    }
    return true;
}

void QrAutoCalib::pose_callback(const::agv_msgs::PoseWithConfidence& msg)
{
    cur_pose_ = msg.pose.pose;
    cur_theta_ = tf::getYaw(msg.pose.pose.orientation);
}

bool QrAutoCalib::qr_calib_callback(agv_srvs::QRCalib::Request &req, agv_srvs::QRCalib::Response &resp)
{
    trigger = req.trigger;
    node0 = req.node0;
    node1 = req.node1;
    node2 = req.node2;
    node3 = req.node3;
    node4 = req.node4;
    node5 = req.node5;
    node6 = req.node6;
    node7 = req.node7;
    node8 = req.node8;
    node9 = req.node9;

    // node0 = 1259;
    // node1 = 1260;
    // node2 = 1262;
    // node3 = 1263;
    // node4 = 1264;
    // node5 = 1265;

    node0_pose.position.x = map_stream_reader_.pointsMap[node0].x;
    node0_pose.position.y = map_stream_reader_.pointsMap[node0].y;
    diff = getdistance(node0_pose,cur_pose_);
    ROS_INFO("call server-----------------trigger:%d", trigger);
    if(trigger==1) {
        if(diff > diff_threshold || agv_navigation_type != 1){
            qr_msg = "please move agv to start node , check agv opration mode and nav mode";
            resp.msg = qr_msg;
            resp.success = 2;
        }
        else if(percent >0 && percent <100){
            qr_msg = "calibrating";
            resp.msg = qr_msg;
            resp.success = 0;
        }
        else{
            agv_srvs::SetOperationMode::Request req;
            agv_srvs::SetOperationMode::Response ret;
            //ros::service::waitForService("/operation_mode");
            req.mode = "auto";
            req.sender = 1;
            req.submode = "local";
            mode_clinet.call(req,ret);
            std::this_thread::sleep_for(std::chrono::seconds(1));
            qr_msg = "start";
            percent = 50;
            moveCycle = 10;
            resp.msg = qr_msg;
            resp.success = 1;
            calib_success_flag = false;
            pose_list.clear();
        }

    }
    else if(trigger == 2) {
        qr_msg = "cancel";
        percent = 0;
        resp.msg = qr_msg;
        resp.success = 1;
        calib_success_flag = false;
        //set_tf(pose_x_old,pose_y_old,pose_theta_old);
         moveDir = 0;
         node1_0_called = false;
         node2_0_called = false;
         node3_0_called = false;
         node4_0_called = false;
         node5_0_called = false;
         node6_0_called = false;
         node7_0_called = false;
         node8_0_called = false;
         node9_0_called = false;
         node9_1_called = false;
         node8_1_called = false;
         node7_1_called = false;
         node6_1_called = false;
         node5_1_called = false;
         node4_1_called = false;
         node3_1_called = false;
         node2_1_called = false;
         node1_1_called = false;
         first_in = false;
         first_start = false;
    }
    else if(trigger == 3) {
        if(calib_success_flag == false && percent <100 && percent >0){
            qr_msg = "calibrating !";
            resp.msg = qr_msg;
            resp.success = 0;

        }
        else if(calib_success_flag == true){
            qr_msg = "calibration finish!";
            resp.angle = pose_theta_;
            resp.msg = qr_msg;
            resp.success = 1;
        }
        else if(calib_success_flag == false && percent == 0){
            qr_msg = "calibration not start!";
            resp.msg = qr_msg;
            resp.success = 2;
        }
        else if(percent < 0){
            qr_msg = "calibration failed!";
            resp.msg = qr_msg;
            resp.success = 2;
        }
    }
    else if(trigger == 4){

    }
    else{
        resp.msg = "request error ";
    }
    return true;
}

void QrAutoCalib::work_cycle()
{
    ROS_INFO_STREAM("work cycle !");
    ros::Rate rate1(1);
    ros::Rate rate2(0.2);
    while(ros::ok())
    {
        //ROS_WARN_STREAM("trigger is "<< &trigger);
        if(trigger == 2){
                ROS_WARN_STREAM_THROTTLE(1,"Calibration Interrupted!");
                rate1.sleep();
                continue;
        }

        if(calib_success_flag == true){
                ROS_INFO_STREAM_THROTTLE(1,"Calibration finished!");
                rate1.sleep();
                continue;
        }
        if(percent < 0){
                ROS_INFO_STREAM_THROTTLE(1,"Calibration failed!");
                rate1.sleep();
                continue;
        }

        if(trigger == 1){
            ROS_WARN_STREAM("diff is "<< diff<<" first in "<<first_in<<" agvMode "<<agvMode<<" agv nav type "<<agv_navigation_type);
            bool move_ret,cal_ret;
            if (diff < diff_threshold && first_in == false && agvMode == 2 && agv_navigation_type == 1){
                //move_ret = line_move(node0,node1);
                first_start = true;
                first_in = true;
            }
            else{
                ROS_WARN_STREAM("please move agv to start node , check agv opration mode and nav mode !");
                rate1.sleep();
                continue;
            }

            while(moveCycle >= 0 && trigger != 2){
                if(moveCycle == 0){
                    percent = -1;
                    //set_tf(pose_x_old,pose_y_old,pose_theta_old);
                    calib_success_flag = false;
                    ROS_INFO_STREAM_THROTTLE(1,"qrCode calibration failed!");
                    break;
                }
                //ROS_INFO_STREAM("moveCycle is "<< moveCycle << " from node "<<fromNode<<" to node "<< toNode << " move dir "<< moveDir << " calib_success_flag "<<calib_success_flag);
                //ROS_INFO_STREAM("first_start "<< first_start << " move_ret "<<move_ret<<" from node "<< fromNode <<" to node "<< toNode << " calib_success_flag "<<calib_success_flag);
                if(first_start == true || (move_ret == true && fromNode == node0 && toNode == node0 && moveDir == 0 && calib_success_flag == false && trigger != 2)){
                    if(calib_success_flag == false && first_start == false){
                        bool ret = yaw_cal();
                        pose_list.clear();
                    }
                    moveDir = 1;
                    node_list.clear();
                    node_list.push_back(node0);
                    node_list.push_back(node1);
                    node_list.push_back(node2);
                    node_list.push_back(node3);
                    node_list.push_back(node4);
                    node_list.push_back(node5);
                    node_list.push_back(node6);
                    node_list.push_back(node7);
                    node_list.push_back(node8);
                    node_list.push_back(node9);
                    move_ret = line_move(node_list);
                    first_start = false;
                    rate2.sleep();
                }
                else if(move_ret == true && fromNode == node9 && toNode == node9 &&  moveDir == 1 && calib_success_flag == false && trigger != 2){
                    //rate2.sleep();
                    if(calib_success_flag == false){
                        bool ret = yaw_cal();
                        pose_list.clear();
                    }
                    moveDir = 0;
                    node_list.clear();
                    node_list.push_back(node9);
                    node_list.push_back(node8);
                    node_list.push_back(node7);
                    node_list.push_back(node6);
                    node_list.push_back(node5);
                    node_list.push_back(node4);
                    node_list.push_back(node3);
                    node_list.push_back(node2);
                    node_list.push_back(node1);
                    node_list.push_back(node0);
                    move_ret = line_move(node_list);
                    ROS_INFO_STREAM("moveCycle "<< moveCycle);
                    moveCycle --;
                    rate2.sleep();
                }
                if(fromNode == node1 && toNode == node2 && moveDir == 1 || fromNode == node8 && toNode == node7 && moveDir == 0){
                    record_start = true;
                }
                if(fromNode == node8 && toNode == node9 && moveDir == 1 || fromNode == node1 && toNode == node0 && moveDir == 0){
                    record_start = false;
                }
                // ROS_WARN_STREAM_THROTTLE(0.1," record_start " << record_start<<" move dir "<<moveDir<<" from node "<<fromNode<<" to node "<<toNode);
                if(trigger == 2){
                    break;
                }
                rate2.sleep();
            }

        }
        rate1.sleep();
    }

}
bool QrAutoCalib::line_move(int fromNode,int toNode)
{
    ROS_INFO_STREAM("line_move-----------------distance");
    //ros::Rate rate(10);
    agv_msgs::V_order order;
    order.orderId = std::to_string(order_num);
    //order.orderUpdateId = order_num;
    agv_msgs::V_node node;
    agv_srvs::V_localOrder::Request req;
    agv_srvs::V_localOrder::Response res;
    node.nodeId = std::to_string(fromNode);
    order.nodes.push_back(node);
    node.nodeId = std::to_string(toNode);
    order.nodes.push_back(node);
    req.order = order;
    req.recycle = false;
    order_client.call(req,res);
    order_num ++;
    ROS_WARN_STREAM(res.message);
    if(res.success == true)return true;
    else return false;
}

bool QrAutoCalib::line_move(vector<int> node_list)
{
    ROS_INFO_STREAM("line_move-----------------nodes");
    //ros::Rate rate(10);
    agv_msgs::V_order order;
    order.orderId = std::to_string(order_num);
    order.orderUpdateId = order_num;
    agv_msgs::V_node node;
    agv_srvs::V_localOrder::Request req;
    agv_srvs::V_localOrder::Response res;
    for(int node_:node_list)
    {
        node.nodeId = std::to_string(node_);
        order.nodes.push_back(node);
        order.zoneSetId = std::to_string(node_);
    }
    req.order = order;
    req.recycle = false;
    order_client.call(req,res);
    order_num ++;
    ROS_WARN_STREAM(res.message);
    if(res.success == true)return true;
    else return false;
}

bool QrAutoCalib::yaw_cal()
{
    ROS_WARN_STREAM("yaw cal !!!!");
    if(pose_list.size()<=0){
        ROS_ERROR_STREAM(" pose list size is 0 ,please check map and qrCode landmark!!!");
        percent = -1;
        return false;
    }
    double delta_x,delta_y,delta_yaw = 0,delta_yaw_ = 0;
    if(map_stream_reader_.pointsMap[node1].x == map_stream_reader_.pointsMap[node9].x){
        for(int i = 0;i<pose_list.size()-1;i++){
            //delta_x = pose_list[i+1].x-pose_list[i].x;
            //ROS_ERROR_STREAM("i is "<< i << " pose list size is "<<pose_list.size());
            delta_x = pose_list[i+1].x-map_stream_reader_.pointsMap[node1].x;
            delta_y = pose_list[i+1].y-pose_list[i].y;
            delta_yaw += atan2(delta_x,abs(delta_y));
            delta_yaw_ += abs(atan2(delta_x,abs(delta_y)));
            //ROS_INFO_STREAM(atan2(delta_x,abs(delta_y)));
        }
        delta_yaw = delta_yaw/(pose_list.size());
        if(abs(cur_theta_ - M_PI_2)<10*M_PI/180){ //车头90度和-90度时，x偏差有正负。
            delta_yaw = delta_yaw;
        }
        else if(abs(cur_theta_ + M_PI_2)<10*M_PI/180){
            delta_yaw = -delta_yaw;
        }

    }
    else if(map_stream_reader_.pointsMap[node1].y == map_stream_reader_.pointsMap[node9].y){
        for(int i = 0;i<pose_list.size()-1;i++){
            //ROS_ERROR_STREAM("i is "<< i << " pose list size is "<<pose_list.size());
            delta_x = pose_list[i+1].x-pose_list[i].x;
            //delta_y = pose_list[i+1].y-pose_list[i].y;
            delta_y = pose_list[i+1].y-map_stream_reader_.pointsMap[node1].y;
            delta_yaw += atan2(delta_y,abs(delta_x));
            delta_yaw_ += abs(atan2(delta_y,abs(delta_x)));
        }
        delta_yaw = delta_yaw/(pose_list.size());
        if(abs(cur_theta_ - M_PI)<=10*M_PI/180 || abs(cur_theta_ + M_PI)<=10*M_PI/180){
            delta_yaw = delta_yaw;
        }
        else if(abs(cur_theta_ - 0)<10*M_PI/180){
            delta_yaw = -delta_yaw;
        }
    }
    //ROS_ERROR_STREAM("delta_yaw "<< delta_yaw);
    delta_yaw_ = delta_yaw_/(pose_list.size());
    ROS_ERROR_STREAM(" pose_theta_ "<< pose_theta_<< " delta_yaw is "<<delta_yaw<<" delta_yaw_abs is "<<delta_yaw_);
    if(abs(delta_yaw_) < 0.002){
        calib_success_flag = true;
        percent = 100;
        ros::param::set("/code_read_camera/scale",scale);
        ROS_INFO_STREAM("calib finish !!!");
        return true;
    }
    else{
        pose_theta_ = pose_theta_ + delta_yaw;
        ros::param::set("/dm_reader/TcR",pose_theta_);
        //ROS_ERROR_STREAM(" pose_theta_ "<< pose_theta_);
    }
    return true;
}

int main(int argc, char** argv) {
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "qrcalib");
    ros::NodeHandle nh("~");
    ros::AsyncSpinner spinner(4);
    QrAutoCalib calibNode_(nh);
    calibNode_.init();
    spinner.start();
    ros::Rate rate(1);
    while(ros::ok()) {
        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}
